#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include "ros2_cpp_pkg/action/move_robot.hpp"


typedef ros2_cpp_pkg::action::MoveRobot MoveRobot;
typedef rclcpp_action::ServerGoalHandle<MoveRobot> MoveRobotGoalHandle;

using std::placeholders::_1;
using std::placeholders::_2;

const float DIS_THRESHOLD = 0.01;   // meters

class MoveRobotActionServerNode: public rclcpp::Node
{
public:
  MoveRobotActionServerNode(): Node("move_robot_action_server_node")
  {
    robot_postion_ = 50;
    cb_group_ = this->create_callback_group(rclcpp::CallbackGroupType::Reentrant);
    action_server_ = rclcpp_action::create_server<MoveRobot>(
      this, "move_robot",
      std::bind(&MoveRobotActionServerNode::handle_goal, this, _1, _2),
      std::bind(&MoveRobotActionServerNode::handle_cancel, this, _1),
      std::bind(&MoveRobotActionServerNode::handle_accepted, this, _1),
      rcl_action_server_get_default_options(),
      cb_group_
    );
    RCLCPP_INFO(this->get_logger(), "Move Robot action server has been started");
    RCLCPP_INFO(this->get_logger(), "Robot postion: %d", robot_postion_);
  }

private:
  rclcpp_action::GoalResponse handle_goal(
    const rclcpp_action::GoalUUID &uuid,
    std::shared_ptr<const MoveRobot::Goal> goal)
  {
    (void)uuid;
    RCLCPP_INFO(this->get_logger(), "Received a new goal");

    // Validata new goal
    if ((goal->position < 0) || (goal->position > 100) || (goal->velocity < 0)) {
      RCLCPP_INFO(this->get_logger(), "Invalid position/velociy, reject goal");
      return rclcpp_action::GoalResponse::REJECT;
    }

    // New goal is valid, preempt previous goal
    {
      std::lock_guard<std::mutex> lock(mutex_);
      if (goal_handle_) {
        if (goal_handle_->is_active()) {
          preempted_goal_id = goal_handle_->get_goal_id();
        }
      }
    }

    // Accept goal
    RCLCPP_INFO(this->get_logger(), "Accept goal");
    return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
  }

  rclcpp_action::CancelResponse handle_cancel(
    const std::shared_ptr<MoveRobotGoalHandle> goal_handle)
  {
    (void)goal_handle;
    RCLCPP_INFO(this->get_logger(), "Recieved request to cancel goal");
    return rclcpp_action::CancelResponse::ACCEPT;
  }

  void handle_accepted(
    const std::shared_ptr<MoveRobotGoalHandle> goal_handle)
  {
    // Start new thread to prevent blocking ROS executor
    execute(goal_handle);
  }

  void execute(const std::shared_ptr<MoveRobotGoalHandle> goal_handle)
  {
    {
      std::lock_guard<std::mutex> lock(mutex_);
      this->goal_handle_ = goal_handle;
    }

    int goal_position = goal_handle->get_goal()->position;
    int velocity = goal_handle->get_goal()->velocity;

    auto feedback = std::make_shared<MoveRobot::Feedback>();
    auto result = std::make_shared<MoveRobot::Result>();
    rclcpp::Rate loop_rate(1); // Rate in Hz to calculate distance, publish feedback
    feedback->current_position = DIS_THRESHOLD; // Init value to trigger while loop
    
    RCLCPP_INFO(this->get_logger(), "Executing Goal");
    // feedback->current_position >= DIS_THRESHOLD
    while (rclcpp::ok())
    {
      // Check if needs to preempt goal
      {
        std::lock_guard<std::mutex> lock(mutex_);
        if (goal_handle->get_goal_id() == preempted_goal_id)
        {
          result->position = robot_postion_;
          result->message = "Preenpted by another goal";
          goal_handle->abort(result);
          return;
        }
      }

      // Check if needs to cancell request
      if (goal_handle->is_canceling())
      {
        result->position = robot_postion_;
        if (goal_position == robot_postion_)
        {
          result->message = "success";
          goal_handle->succeed(result);
        }
        else {
          result->message = "Canceled";
          goal_handle->canceled(result);
        }
        return;
      }

      float diff = (float)(goal_position - robot_postion_);

      if (abs(diff) < DIS_THRESHOLD)
      {
        result->position = robot_postion_;
        result->message = "Success";
        goal_handle->succeed(result);
        return;
      }
      else if (diff > 0)
      {
        if (diff < velocity) 
        {
          robot_postion_ += diff;
        }
        else {
          robot_postion_ += velocity;
        }
      }
      else if (diff < 0)
      {
        if (abs(diff) < velocity) 
        {
          robot_postion_ -= abs(diff);
        }
        else {
          robot_postion_ -= velocity;
        }
      }
      
      RCLCPP_INFO(this->get_logger(), "Robot position: %d", robot_postion_);
      feedback->current_position = robot_postion_;
      goal_handle->publish_feedback(feedback);

      loop_rate.sleep();
    }
    RCLCPP_INFO(this->get_logger(), "Goal succeeded");
  }

  std::mutex mutex_;
  std::shared_ptr<MoveRobotGoalHandle> goal_handle_;
  rclcpp_action::GoalUUID preempted_goal_id;
  rclcpp_action::Server<MoveRobot>::SharedPtr action_server_;
  rclcpp::CallbackGroup::SharedPtr cb_group_;
  int robot_postion_;
};


int main(int argc, char *argv[])
{
  rclcpp::init(argc, argv);

  auto move_robot_server =  std::make_shared<MoveRobotActionServerNode>();

  rclcpp::executors::MultiThreadedExecutor executer;
  executer.add_node(move_robot_server);
  executer.spin();
  // rclcpp::spin(move_robot_server);
  rclcpp::shutdown();

  return 0;
}